#! /opt/local/bin/perl5.12.3
package Astar;

use strict;
use warnings;

sub uniformCostSearch {
    my ($problem, $properties, $root, $goal) = (@_);
    my ($cityName, $cityCoords) = (0, 1);
    my ($x, $y) = (0, 1);
    my ($u, $minCost, $minCostPos, $minCostFinal);
    my ($childPosition, $toChildCost, $childName);

    my $root_name = $root->[$cityName];
    my $goal_name = $goal->[$cityName];

    return [] if $root_name eq $goal_name;
    my %explored = ();  # [parent, coord, cost]
    $explored{$root_name} = [undef, $root->[$cityCoords], 0];
    my @frontiers = ();
    push @frontiers, $root_name;

    while ( @frontiers ) {
        $u = shift @frontiers;
        $minCostFinal = 0+"inf";
        for my $neighbour ( @{ $problem->{$u} } ) {
            ($childPosition, $toChildCost, $childName) = map{ $_ } @$neighbour;
            if ( not defined $explored{$childName} ) {
                $minCost = $explored{$u}[2] + $toChildCost;
                $minCostPos = $childPosition;
                if ( $minCost < $minCostFinal ) {
                    $minCostFinal = $minCost;
                    $minCostPos = $childPosition;   }
                $explored{$childName} = [$u, $childPosition, $explored{$u}[2] + $toChildCost];
                push @frontiers, $childName;
                if ($childName eq $goal->[$cityName]) {
                    return buildPath(\%explored, $goal, $root);     } } } }
}

sub buildPath {
    my ($explored, $goal, $root) = (@_);
    my $cityName = 0;
    my $root_name = $root->[$cityName];
    my $goal_name = $goal->[$cityName];

    my $path = [];
    my $parent = 0;
    my $coordinate = 1;
    my $totalCost = $explored->{$goal_name}[2];

    push @{$explored->{$goal_name}[$coordinate]}, "$goal_name";
    unshift @$path, $explored->{$goal->[$cityName]}[$coordinate];

    my $current = $explored->{$goal->[$cityName]}[$parent];
    while ( $current ne $root->[$cityName] ) {
        push @{$explored->{$current}[$coordinate]}, "$current";
        #print STDOUT "TESTPRINT: $explored->{$current}[$coordinate][2]\n";
        unshift @$path, $explored->{$current}[$coordinate];
        $current = $explored->{$current}[$parent];
    }
    push @{$explored->{$root_name}[$coordinate]}, "$root_name";
    unshift @$path, $explored->{$root->[$cityName]}[$coordinate];
    return ($path, $totalCost);
}


1;
